#ifndef __OCCUPANCY_MAP_H_
#define __OCCUPANCY_MAP_H_

#include <slam_2d_ros/common/slam_define.h>
#include <slam_2d_ros/common/frame.h>

namespace slam_2d{

class OccupancyMap{
    public:
        /// 栅格模板，预先计算
        struct Model2DPoint {
            int dx_ = 0;
            int dy_ = 0;
            double angle_ = 0;  // in rad
            float range_ = 0;   // in meters
        };

        /// 从世界坐标系转到图像坐标系
        // template <class T>
        inline Vec2i World2Image(const Vec2d& pt) {
            Vec2d pt_map = (pose_.inverse() * pt) * resolution_ + center_image_;
            int x = int(pt_map[0]);
            int y = int(pt_map[1]);
            return Vec2i(x, y);
        }

    public:

        // 构造函数
        OccupancyMap();
        ~OccupancyMap();

        // 插入一帧雷达数据
        void AddLidarFrame(boost::shared_ptr<Frame> frame);
        // 设置pose 
        void SetPose(const Pose2D& pose);
        // 设置点值 见P219
        void SetPoint(const Vec2i& pt, bool occupy);
        // 直线填充
        void BresenhamFilling(const Vec2i& p1, const Vec2i& p2);
        // 获取地图数据
        cv::Mat GetOccupancyGridBlackWhite() const;


        // 获取原始的占据栅格地图
        cv::Mat GetOccupancyGrid() const { 
            return occupancy_grid_; 
        }
        bool HasOutsidePoints() const { 
            return has_outside_pts_; 
        }

    private:
        static constexpr double resolution_ = 20.0;        // 1m 多少像素
        static constexpr float inv_resolution_ = 0.05;     // 1个像素多少米（栅格分辨率）
        static constexpr int image_size_ = 1000;           // 图像大小
        static constexpr int model_size_ = 400;            // 模板像素大小

        Pose2D pose_ = Eigen::Isometry2d::Identity();  // T_W_S
        cv::Mat occupancy_grid_;  // 8bit 占据栅格图像
        Vec2d center_image_;
        bool has_outside_pts_ = false;  // 标注栅格化过程中是否有落在外部的点
        // 模板
        std::vector<Model2DPoint> model_;  // 用于填充占据栅格的模板，都是世界系下的点
};

};   //namespace slam_2d

#endif